﻿using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using HardwareInterface;
using HardwareInterface.Platform;
using OpenCvSharp;
using Point = OpenCvSharp.Point;
using Size = OpenCvSharp.Size;

namespace EMGunSharp
{
    class CircleTargetTracker : IDisposable
    {
        Camera cam;
        bool runflag = true;
        public event Action<Target> TargetFoundEvent;
        public event Action TargetGoneEvent;

        public struct Target
        {
            public float X, Y, R;
        }

        /// <summary>
        /// 每秒处理的图像数量
        /// <para>剩余图像被丢弃</para>
        /// </summary>
        public int FPS { private set; get; }

        /// <summary>
        /// 红色圆圈追踪器
        /// </summary>
        /// <param name="camId">摄像机编号</param>
        public CircleTargetTracker(int camId = 0, bool usbcam = false)
        {
            if (Platform.IsJetson() && !usbcam)
                cam = new Camera(new Camera.NvArgusCameraSrc.NvCamSettings
                {
                    CamNumber = camId,
                    Flip = Camera.NvArgusCameraSrc.NvCameraFlip.No,
                    Framerate = 20,
                    Height = 480,
                    Width = 640
                });
            else
                cam = new Camera(camId,VideoCaptureAPIs.V4L2);
            cam.cap.Fps = 20;
            cam.Init();
            int c = 0;
            new Thread(new ThreadStart(() =>
            {
                bool lastfound = false;
                Mat frame = cam.GetLatestFrame().Clone();
                while (runflag)
                {
                    try
                    {
                        frame = cam.GetLatestFrame();
                        Cv2.Resize(frame, frame, new Size(640, 480));
                        Mat binframe = R2Gary(frame).Clone();//通道抑制，获取红色区域
                        Cv2.AdaptiveThreshold(binframe, binframe, 200, AdaptiveThresholdTypes.MeanC, ThresholdTypes.Binary, 9, 1);//增强二值化

                        //结构化,散点聚合
                        Mat kernel = Cv2.GetStructuringElement(MorphShapes.Rect, new Size(3, 3), new Point(-1, -1));
                        Cv2.MorphologyEx(binframe, binframe, MorphTypes.Close, kernel, new Point(-1, -1));
                        kernel = Cv2.GetStructuringElement(MorphShapes.Rect, new Size(3, 3), new Point(-1, -1));
                        Cv2.MorphologyEx(binframe, binframe, MorphTypes.Open, kernel, new Point(-1, -1));

                        Cv2.FindContours(binframe, out Point[][] conts, out HierarchyIndex[] h, RetrievalModes.List, ContourApproximationModes.ApproxNone);//分离不连续的散点,分离物体
                        bool found = false;
                        //Window.ShowImages(new Mat[] { binframe });
                        foreach (var shape in conts)
                        {
                            if (!Circle(shape, out Point center, out double R)) continue;//是圆?
                            if (R < 10) continue;//太小
                            TargetFoundEvent?.Invoke(new Target
                            {
                                X = center.X - (binframe.Width / 2),
                                Y = (binframe.Height / 2) - center.Y,
                                R = (float)R
                            });
                            lastfound = true;
                            found = true;
                            break;//算法消耗大，圆一个就够了
                        }
                        if ((!found) && lastfound)
                        {
                            TargetGoneEvent?.Invoke();
                        }
                        lastfound = found;
                    }
                    catch (Exception err)
                    {
                        Console.WriteLine(err.Message);
                        Console.WriteLine(err.StackTrace);
                    }
                    c++;
                }
            })).Start();


            //帧数统计
            new Thread(new ThreadStart(() =>
            {
                while (runflag)
                {
                    Thread.Sleep(1000);
                    FPS = c;
                    c = 0;
                }
            })).Start();
        }

        /// <summary>
        /// 使用密集点集确认圆心和半径
        /// </summary>
        /// <param name="pg">点的集合</param>
        /// <param name="center">圆心位置</param>
        /// <param name="R">半径</param>
        /// <param name="centerth">允许点偏离圆心的最大比例</param>
        /// <param name="singleth">允许点集中不在圆上的点的比例</param>
        /// <returns>是否成功拟合出圆</returns>
        public bool Circle(Point[] pg, out Point center, out double R, double centerth = 0.1, double singleth = 0.3)
        {
            Point Left = pg[0], Up = pg[0], Right = pg[0], Down = pg[0];
            foreach (var p in pg)
            {
                if (p.X < Left.X) Left = p;
                if (p.X > Right.X) Right = p;
                if (p.Y < Down.Y) Down = p;
                if (p.Y > Up.Y) Up = p;
            }
            center = new Point((Left.X + Right.X) / 2, (Up.Y + Down.Y) / 2);
            R = ((Right.X - Left.X) + (Up.Y - Down.Y)) / 4;//平均半径
            int miss = 0;
            int total = 0;
            foreach (var p in pg)
            {
                var d2d = p - center;
                double distance = Math.Sqrt((d2d.X * d2d.X) + (d2d.Y * d2d.Y));
                if (Math.Abs(distance - R) > R * 0.1) miss++;
                total++;
            }
            return (miss < (total * 0.4));
        }

        /// <summary>
        /// 使用惩罚算法抑制非红色像素并生成灰度图
        /// </summary>
        /// <param name="input">彩色图像</param>
        /// <returns>红色通道灰度图</returns>
        public Mat R2Gary(Mat input)
        {
            var output = Cv2.Split(input);
            var red = output[2];
            var green = output[1];
            var blue = output[0];
            var reducedred = red - green - blue;
            return reducedred;
        }

        public void Dispose()
        {
            runflag = false;
            cam.Abort();
        }
    }
}
